
// sudo ./b2_sport_client enx000ec62796bd
// 192.168.123.161
#include <string>
#include "chassis.h"
#include <unitree/robot/go2/sport/sport_client.hpp>
#include <iostream>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/common/thread/thread.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
#include "unitree_b2.h"
// #include "serial_driver.h"

UnitreeB2* pointer_b2 = nullptr;
bool chassisInit(std::string &dev) {
    pointer_b2 = new UnitreeB2("test");
    unitree::common::ThreadPtr threadPtr = unitree::common::CreateRecurrentThread(pointer_b2 ->dt_ * 1000000, std::bind(&UnitreeB2::RobotControl, pointer_b2));
    return true;
}

bool getVersion(char *data) {
    return true;
}

bool setText(const char *text) {
    return true;
}

bool updateChassisInfo(ChassisData *data) {
    // update control cmd
    pointer_b2->data_.aim_x_vel = data->aim_x_vel;
    pointer_b2->data_.aim_y_vel = data->aim_y_vel;
    pointer_b2->data_.aim_z_vel = data->aim_z_vel;

    pointer_b2->sport_client_.Move(data->aim_x_vel, data->aim_y_vel, data->aim_z_vel);

    // update B2 info
    data->odom_x = pointer_b2->data_.odom_x;
    data->odom_z = pointer_b2->data_.odom_z;
    data->odom_y = pointer_b2->data_.odom_y;
    data->vel_x = pointer_b2->data_.vel_x;
    data->vel_z = pointer_b2->data_.vel_z;
    data->vel_y =pointer_b2->data_.vel_y;
    data->voltage =pointer_b2->data_.voltage;
    data->current = pointer_b2->data_.current;
    return true;
}

bool setSysCmd(uint32_t cmd) {
    return true;
}

bool setIo(uint32_t io) {
    return true;
}
